#include "Engine/StdAfx.h"
#include "Quat.h"
#include "Mat33.h"

namespace fastbird
{
	const float Quat::ms_fEpsilon = 1e-03f;
	const Quat Quat::ZERO(0, 0, 0, 0);
	const Quat Quat::IDENTITY(1, 0, 0, 0);

	void Quat::FromRotationMatrix(const Mat33& rot)
	{
		// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
        // article "Quaternion Calculus and Fast Animation".

        float fTrace = rot[0][0]+rot[1][1]+rot[2][2];
        float fRoot;

        if ( fTrace > 0.0 )
        {
            // |w| > 1/2, may as well choose w > 1/2
            fRoot = sqrt(fTrace + 1.0f);  // 2w
            w = 0.5f*fRoot;
            fRoot = 0.5f/fRoot;  // 1/(4w)
            x = (rot[2][1]-rot[1][2])*fRoot;
            y = (rot[0][2]-rot[2][0])*fRoot;
            z = (rot[1][0]-rot[0][1])*fRoot;
        }
        else
        {
            // |w| <= 1/2
            static size_t s_iNext[3] = { 1, 2, 0 };
            size_t i = 0;
            if ( rot[1][1] > rot[0][0] )
                i = 1;
            if ( rot[2][2] > rot[i][i] )
                i = 2;
            size_t j = s_iNext[i];
            size_t k = s_iNext[j];

            fRoot = sqrt(rot[i][i]-rot[j][j]-rot[k][k] + 1.0f);
            float* apkQuat[3] = { &x, &y, &z };
            *apkQuat[i] = 0.5f*fRoot;
            fRoot = 0.5f/fRoot;
            w = (rot[k][j]-rot[j][k])*fRoot;
            *apkQuat[j] = (rot[j][i]+rot[i][j])*fRoot;
            *apkQuat[k] = (rot[k][i]+rot[i][k])*fRoot;
        }
	}
}